#include "roombalib.h"
#include "opencv/highgui.h"
#include "opencv/cv.h"
#include "ivy.h"
#include "ivyloop.h"
#include <sys/ioctl.h>
#include <termios.h>
#include <pthread.h>
#include <iostream>
#include <getopt.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <errno.h>
#include <time.h>
#include <math.h>

#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define sign(x) ((x) > 0 ? 1 : -1)
#define STEP_MIN 5
#define STEP_MAX 100

IplImage *image;
CvPoint objectNextPos;
int nbPixels;
CvPoint objectPos = cvPoint(-1, -1);
int tolerance = 10;
int h=19, s=191, v=93;		//Orange, ça sera vert
int t_marker[4];
typedef struct {
	int x;
	int y;
	int height;
	int width;
	int tab[4];
	IplImage* image;
	CvPoint objectNextPos;
	int nbPixels;
}	DATA;

	pthread_mutex_t mutex;

CvPoint binarisation(IplImage* image, int *nbPixels) {
	int x, y;
	CvScalar pixel;
	IplImage *hsv, *mask;
	IplConvKernel *kernel;
	int sommeX = 0, sommeY = 0;
	*nbPixels = 0;
	mask = cvCreateImage(cvGetSize(image), image->depth, 1);
	hsv = cvCloneImage(image);
	cvCvtColor(image, hsv, CV_BGR2HSV);
	cvInRangeS(hsv, cvScalar(h - tolerance -1, s - tolerance, 0), cvScalar(h + tolerance -1, s + tolerance, 255), mask);
	kernel = cvCreateStructuringElementEx(5, 5, 2, 2, CV_SHAPE_ELLIPSE);
	cvDilate(mask, mask, kernel, 1);
	cvErode(mask, mask, kernel, 1);
	for(x = 0; x < mask->width; x++) {
		for(y = 0; y < mask->height; y++) {
			if(((uchar *)(mask->imageData + y*mask->widthStep))[x] == 255) {
				sommeX += x;
				sommeY += y;
				(*nbPixels)++;
			}
		}
	}
	cvReleaseStructuringElement(&kernel);
	cvReleaseImage(&mask);
   	cvReleaseImage(&hsv);
	if(*nbPixels > 0)
		return cvPoint((int)(sommeX / (*nbPixels)), (int)(sommeY / (*nbPixels)));
	else
		return cvPoint(-1, -1);
}

void *marker() {
	int objectNextStepX, objectNextStepY;
	if (nbPixels > 10) {
		if (objectPos.x == -1 || objectPos.y == -1) {
			objectPos.x = objectNextPos.x;
			objectPos.y = objectNextPos.y;
		}
		if (abs(objectPos.x - objectNextPos.x) > STEP_MIN) {
			objectNextStepX = max(STEP_MIN, min(STEP_MAX, abs(objectPos.x - objectNextPos.x) / 2));
			objectPos.x += (-1) * sign(objectPos.x - objectNextPos.x) * objectNextStepX;
		}
		if (abs(objectPos.y - objectNextPos.y) > STEP_MIN) {
			objectNextStepY = max(STEP_MIN, min(STEP_MAX, abs(objectPos.y - objectNextPos.y) / 2));
			objectPos.y += (-1) * sign(objectPos.y - objectNextPos.y) * objectNextStepY;
		}

		pthread_mutex_lock (&mutex);
		t_marker[0] = objectPos.x;
		t_marker[1] = objectPos.y;
		t_marker[2] = cvGetSize(image).height;
		t_marker[3] = cvGetSize(image).width;
		pthread_mutex_unlock (&mutex);

		printf("x = %d, y = %d \n",objectPos.x, objectPos.y);
		printf("h = %d, l = %d \n",cvGetSize(image).height, cvGetSize(image).width);
	}
	else {
		objectPos.x = -1;
		objectPos.y = -1;
	}
	if (nbPixels > 10)
		cvDrawCircle(image, objectPos, 15, CV_RGB(127, 127, 127), -1);

	cvDrawLine(image,cvPoint((cvGetSize(image).width)/3,0),cvPoint((cvGetSize(image).width)/3,cvGetSize(image).height),CV_RGB(255,0,0),1,8,0);
	cvDrawLine(image,cvPoint((cvGetSize(image).width)/1.5,0),cvPoint((cvGetSize(image).width)/1.5,cvGetSize(image).height),CV_RGB(255,0,0),1,8,0);
	cvDrawLine(image,cvPoint(0,(cvGetSize(image).height)/3),cvPoint(cvGetSize(image).width,(cvGetSize(image).height)/3),CV_RGB(255,0,0),1,8,0);
	cvDrawLine(image,cvPoint(0,(cvGetSize(image).height)/1.5),cvPoint(cvGetSize(image).width,(cvGetSize(image).height)/1.5),CV_RGB(255,0,0),1,8,0);
	cvShowImage("Suivi de couleur", image);
}

void *roombamove(void *parm) {

	pthread_mutex_lock (&mutex);
	int PosX = t_marker[0];
	int PosY = t_marker[1];
	int ImageHeight = t_marker[2];
	int ImageWidth  = t_marker[3];
	pthread_mutex_unlock (&mutex);

	Roomba* roomba = roomba_init( "/dev/ttyUSB0", B115200 );
	/**-------------------------------------------------------------Ligne1----------------------------------------------------------------------*/
	if(PosX >= 0 && PosX <= (ImageWidth)/3 && PosY >= 0 && PosY <= (ImageHeight)/3 ){
		//DriveCase1
		roomba_drive( roomba, 50, 200 );
		roomba_delay(300);
	}

	else if(PosX >= (ImageWidth)/3 && PosX <= (ImageWidth)/1.5 && PosY >= 0 && PosY <= (ImageHeight)/3 ){
		//DriveCase2
		roomba_drive( roomba, 50, 0 );
		roomba_delay(270);
	}

	else if(PosX >= (ImageWidth)/1.5 && PosX <= (ImageWidth) && PosY >= 0 && PosY <= (ImageHeight)/3 ){
		//DriveCase3
		roomba_drive( roomba, 50, -200 );
		roomba_delay(300);
	}
	/**-------------------------------------------------------------Ligne2----------------------------------------------------------------------*/

	else if(PosX >= 0 && PosX <= (ImageWidth)/3 && PosY >= (ImageHeight)/3 && PosY <= (ImageHeight)/1.5 ){
		//DriveCase4
		roomba_drive( roomba, 50, 0 );
		roomba_delay(250);
		roomba_drive( roomba, 50, 1 );
		roomba_delay(200);
	}

	else if(PosX >= (ImageWidth)/3 && PosX <= (ImageWidth)/1.5 && PosY >= (ImageHeight)/3 && PosY <= (ImageHeight)/1.5 ){
		//DriveCase5
		roomba_stop( roomba );
	// le code a mettre apres l'arossage
	}

	else if(PosX >= (ImageWidth)/1.5 && PosX <= (ImageWidth) && PosY >= (ImageHeight)/3 && PosY <= (ImageHeight)/1.5 ){
		//DriveCase6
		roomba_drive( roomba, 50, 0 );
		roomba_delay(250);
		roomba_drive( roomba, 50, -1 );
		roomba_delay(200);
	}
	/**-------------------------------------------------------------Ligne3----------------------------------------------------------------------*/
	else if(PosX >= 0 && PosX <= (ImageWidth)/3 && PosY >= (ImageHeight)/1.5 && PosY <= (ImageHeight) ){
		//DriveCase7
		roomba_drive( roomba, -50, 0 );
		roomba_delay(200);
	}

	else if(PosX >= (ImageWidth)/3 && PosX <= (ImageWidth)/1.5 && PosY >= (ImageHeight)/1.5 && PosY <= (ImageHeight) ){
		//DriveCase8
		roomba_drive( roomba, -50, 0 );
		roomba_delay(200);
	}

	else if(PosX >= (ImageWidth)/1.5 && PosX <= (ImageWidth) && PosY >= (ImageHeight)/1.5 && PosY <= (ImageHeight) ){
		//DriveCase9
		roomba_drive( roomba, -50, 0 );
		roomba_delay(200);
	}
	/**-----------------------------------------------------------DéplacementAléatoire----------------------------------------------------------*/
	else {
		int r;
		r = rand()%4;
		switch (r) {
		
			case 0 :
				if(roombadebug)
					printf("roomba_forward\n");
				roomba_forward( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				break;
			case 1 :
				if(roombadebug)
					printf("roomba_backward\n");
				roomba_backward( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				break;
			case 2 :
				if(roombadebug)
					printf("roomba_spinleft\n");
				roomba_spinleft( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				break;
			case 3 :
				if(roombadebug)
					printf("roomba_spinright\n");
				roomba_spinright( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				break;
		}
	}
}

int roombamain(int Id) {
	DATA *data1;
	pthread_t thread;
	pthread_create(&thread, NULL, roombamove, NULL);
	IplImage *hsv;
	CvCapture *capture;
	char key;
 	capture = cvCreateCameraCapture(1);
   	if (!capture) {
   		printf("Impossible d'initialiser la capture vidéo.\n");
        return -1;
 	}
   	cvNamedWindow("Suivi de couleur", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Suivi de couleur", 100, 100);
	time_t start,end;
	time(&start);
	int counter=0;
	switch (id) {
		case 0 :
			h=19, s=191, v=93;		//vert
			break;
		case 1 :
			h=19, s=191, v=93;		//violet
			break;
		case 2 :
			h=19, s=191, v=93;		//jaune
			break;
		default :
			h=19, s=191, v=93;		//vert
	}
	while(key != 'Q' && key != 'q') {
		image = cvQueryFrame(capture);
		if(!image)
			continue;
		objectNextPos = binarisation(image, &nbPixels);
		marker();
		time(&end);
		++counter;
		double sec=difftime(end,start);
		double fps=counter/sec;
		printf("\nfps = %lf\n",fps);
		key = cvWaitKey(10);
	}
	cvDestroyWindow("Suivi de couleur");
	cvReleaseCapture(&capture);
	//	pthread_join(thread, NULL);
	return 0;
}

void retrieveMSG (IvyClientPtr app, void *data, int argc, char **argv){
	const char* arg = (argc < 1) ? "" : argv[0];
	printf("argv[0] : %s\n",argv[0]);
	int idcouleur;
	idcouleur = atoi (argv[0]);
	roombamain(idcouleur);
}

main (int argc, char**argv){
	const char* bus = 0;
	char c;
	while (c = getopt (argc, argv, "b:") != EOF) {
		switch (c) {
			case 'b':
				bus = optarg;
				break;
		}
	}
	if (!bus)
		bus = getenv ("IVYBUS");
	IvyInit ("roombavideo", "ICI_roombavideo", 0, 0, 0, 0);		//	initialisation
	IvyStart ("127.255.255.255:2010");							//	On lance l'agent sur le bus ivy
	IvyBindMsg (retrieveMSG, 0, "^S:(.*):(.*)$");				// on récupère tout les message commençant par S:Id_couleur:Qte_eau
	IvyMainLoop();												//	main loop
}
